package simulator;
import java.util.*;

/**
 * This class represents the physical entity of the robot -- and provides direct
 * access to all of the sensors on the board.  It is important to note that
 * controls to the robot are *not* implemented here, but in {@link Simulator},
 * for "historical" reasons.
 *
 * @see Simulator
 */
public class Robot {
  // private fields
 
  protected ArrayList<Sensor> sensors = new ArrayList<Sensor>();
  protected Servo mountplatform;
 private class Command{
    public static final int FORWARD = 0;
    public static final int LEFT = 0;
    public static final int RIGHT = 1;
    public static final int BACKWARD = 1;
    
    public boolean turning;
    public int direction;
    public int arg;
    public Command(boolean turn, int dir, int arg) {
      this.turning = turn;
      this.direction = dir;
      this.arg = arg;
    }

    public Command(){}
  }
  
  private Queue<Command> commands = new LinkedList<Command>();
  
  public Robot() {
  }

  private Command currentCommand = new Command();
  
  public boolean act(Simulator s) {
    if(commands.peek() == null) return false;
    Command c = commands.poll();
    if(c.turning) {
      if(c.direction == Command.LEFT) {
      	s.turnLeft();
      } else {
        s.turnRight();
      }
      s.waitAngle(c.arg);
    } else {
      if(c.direction == Command.FORWARD) {
      	s.goForward();
      } else {
      	s.goBackward();
      }
      s.waitDistance(c.arg);
    }
    return true;
  }
  
  public void turnLeft(int degrees){
	  turnLeft();
	  waitAngle(degrees);
  }
  
  public void turnLeft() {
    currentCommand.direction=Command.LEFT;
    currentCommand.turning=true;
  }

  public void turnRight() {
    currentCommand.direction = Command.RIGHT;
    currentCommand.turning = true;

  }
  
  public void turnRight(int degrees){
	  turnRight();
	  waitAngle(degrees);
  }

  public void goForward() {
    currentCommand.direction = Command.FORWARD;
    currentCommand.turning = false;
  }

  public void goBackward() {
    currentCommand.direction = Command.BACKWARD;
    currentCommand.turning = false;
  }

  
  public void setSpeed() {
    // set speed?
  }

  public void waitAngle(int angle) { 
    currentCommand.arg=angle;
    commands.add(currentCommand);
    currentCommand=new Command();
    
  }

  public void waitDistance(int dist) {
    currentCommand.arg=dist;
    commands.add(currentCommand);
    currentCommand=new Command();
  }


  /**
   * Provides access to a list configured sensors on the robot
   * 
   * @returns an array of {@link Sensor} objects
   */
  public ArrayList<Sensor> getSensors(){
    return sensors;
  }
  public RangeFinder[] getRangeFinders(){

    ArrayList<RangeFinder> temp = new ArrayList<RangeFinder>();
    for(Sensor s: sensors){
      if(s.getClass().toString().equals(new RangeFinder(null, 0.0, 0.0, 0.0).getClass().toString()))
        temp.add((RangeFinder) s);
    }
    RangeFinder[] returnval =new RangeFinder[temp.size()];
    for(int i=0; i<returnval.length; i++){
      returnval[i]= temp.get(i);
    }
    temp=null;
    return returnval;

  }

  public Servo[] getServos(){

    ArrayList<Servo> temp = new ArrayList<Servo>();
    for(Sensor s: sensors){
      if(s.getClass().toString().equals(new Servo(null, null, 0).getClass().toString()))
        temp.add((Servo) s);
    }
    Servo[] returnval =new Servo[temp.size()];
    for(int i=0; i<returnval.length; i++){
      returnval[i]= temp.get(i);
    }
    temp=null;
    return returnval;

  }
 /**
   * Adds another sensor to the robot
   *
   * @param s the {@link Sensor} to be added
   */
  public void addSensor(Sensor s){
    sensors.add(s);
  }

  /**
   * Reads values from all sensors and returns the updated sensors.  This is
   * called internally from Simulator, and therefore should not be accessed by
   * actors
   */
  Sensor[] updateSensors() {
    for(Sensor s: sensors) {
      s.read(68558655);               
    }
    Sensor[] s = new Sensor[sensors.size()];

    for(int i=0; i<sensors.size(); i++) {
      s[i] = sensors.get(i);
    }
    return s;
  }
  
  public void setServo(Servo t){
    mountplatform = t;
  }
  
  public Servo getServo(){
    return mountplatform;
  }
  
  	  
}